#include "vehicle_lateral_tire.h"

namespace auto_ros
{
namespace control
{
VehicleLateralTire::VehicleLateralTire(std::string dyn_yaml_path_name)
	: VehicleDynBase(dyn_yaml_path_name, 4, 1, 3, 1)
{
	TireModelFactory tire_factory;
	auto_ros::common_tool::double_map tire_f_parm = {{"mu", auto_ros::common_tool::find_in_map<double>("mu", parm_)}, {"c_alpha", auto_ros::common_tool::find_in_map<double>("cf", parm_)}};
	auto_ros::common_tool::double_map tire_r_parm = {{"mu", auto_ros::common_tool::find_in_map<double>("mu", parm_)}, {"c_alpha", auto_ros::common_tool::find_in_map<double>("cr", parm_)}};
	tire_f_ptr_ = tire_factory.CreateSharedObject("BrushModel", tire_f_parm);
	tire_r_ptr_ = tire_factory.CreateSharedObject("BrushModel", tire_r_parm);
	mass_ = auto_ros::common_tool::find_in_map<double>("mass", parm_);
	cf_ = auto_ros::common_tool::find_in_map<double>("cf", parm_);
	cr_ = auto_ros::common_tool::find_in_map<double>("cr", parm_);
	lf_ = auto_ros::common_tool::find_in_map<double>("lf", parm_);
	lr_ = auto_ros::common_tool::find_in_map<double>("lr", parm_);
	iz_ = auto_ros::common_tool::find_in_map<double>("iz", parm_);
	max_lateral_acc_ = auto_ros::common_tool::find_in_map<double>("max_lateral_acc", parm_);
	max_tire_f_angle_ = auto_ros::common_tool::find_in_map<double>("max_tire_f_angle", parm_);
	speed_threshold_ = auto_ros::common_tool::find_in_map<double>("speed_threshold", parm_);

	auto_ros::common_tool::double_map input = {{"alpha_rad", 0.0}, {"fz", 4800.0}};
	dyn_cf_ = tire_f_ptr_->calc_dyn_c_alpha(input);
	dyn_cr_ = tire_r_ptr_->calc_dyn_c_alpha(input);
	delta_cf_ = -tire_f_ptr_->calc_static_c_alpha(input) + dyn_cf_;
	delta_cr_ = -tire_r_ptr_->calc_static_c_alpha(input) + dyn_cr_;

	c_yaw_rate_ = -lf_ / 1.0 * dyn_cf_ + lr_ / 1.0 * dyn_cr_;
	c_beta_ = -(dyn_cf_ + dyn_cr_);
	c_delta_ = dyn_cf_;

	d_yaw_rate_ = -lf_ * lf_ / 1.0 * dyn_cf_ - lr_ * lr_ / 1.0 * dyn_cr_;
	d_beta_ = -lf_ * dyn_cf_ + lr_ * dyn_cr_;
	d_delta_ = lf_ * dyn_cf_;

	speed_threshold_ = 1.0;
	//init speed is 1.0m/s
	state_matrix_ << c_beta_ / mass_ / 1.0, c_yaw_rate_ / mass_ - 1.0, 0, 0,
		d_beta_ / iz_ / 1.0, d_yaw_rate_ / iz_, 0, 0,
		1, 0, 0, 1.0,
		0, 1, 0, 0;
	control_matrix_ << c_delta_ / mass_,
		d_delta_ / iz_,
		0,
		0;
	distur_matrix_ << dyn_cf_ / mass_, dyn_cr_ / mass_, 0,
		lf_ * delta_cf_ / iz_, -lr_ * delta_cr_ / iz_, 0,
		0, 0, 0,
		0, 0, -1.0;
	out_state_matrix_ << 0, 0, 1, 0;
	out_distur_matrix_ << 0, 0, 0;
}
void VehicleLateralTire::para_update_matrix(auto_ros::common_tool::double_map &input)
{
	double speed = auto_ros::common_tool::find_in_map<double>("speed", input);
	double alpha_f_rad = auto_ros::common_tool::find_in_map<double>("alpha_f_rad", input);
	double alpha_r_rad = auto_ros::common_tool::find_in_map<double>("alpha_r_rad", input);
	if (std::abs(speed) < speed_threshold_)
	{
		if (speed >= 0)
			speed = speed_threshold_;
		else
			speed = -speed_threshold_;
	}
	auto_ros::common_tool::double_map f_input = {{"alpha_rad", alpha_f_rad}, {"fz", 4800.0}};
	auto_ros::common_tool::double_map r_input = {{"alpha_rad", alpha_r_rad}, {"fz", 4800.0}};
	dyn_cf_ = std::min(tire_f_ptr_->calc_dyn_c_alpha(f_input), -1000.0);
	dyn_cr_ = std::min(tire_r_ptr_->calc_dyn_c_alpha(r_input), -1000.0);
	delta_cf_ = -tire_f_ptr_->calc_static_c_alpha(f_input) + dyn_cf_;
	delta_cr_ = -tire_r_ptr_->calc_static_c_alpha(r_input) + dyn_cr_;

	c_yaw_rate_ = -lf_ / speed * dyn_cf_ + lr_ / speed * dyn_cr_;
	c_beta_ = -(dyn_cf_ + dyn_cr_);
	c_delta_ = dyn_cf_;

	d_yaw_rate_ = -lf_ * lf_ / speed * dyn_cf_ - lr_ * lr_ / speed * dyn_cr_;
	d_beta_ = -lf_ * dyn_cf_ + lr_ * dyn_cr_;
	d_delta_ = lf_ * dyn_cf_;

	state_matrix_ << c_beta_ / mass_ / speed, c_yaw_rate_ / mass_ - speed, 0, 0,
		d_beta_ / iz_ / speed, d_yaw_rate_ / iz_, 0, 0,
		1, 0, 0, speed,
		0, 1, 0, 0;
	control_matrix_ << c_delta_ / mass_,
		d_delta_ / iz_,
		0,
		0;
	distur_matrix_ << dyn_cf_ / mass_, dyn_cr_ / mass_, 0,
		lf_ * delta_cf_ / iz_, -lr_ * delta_cr_ / iz_, 0,
		0, 0, 0,
		0, 0, -speed;
}

double VehicleLateralTire::mass()
{
	return mass_;
}
double VehicleLateralTire::lf()
{
	return lf_;
}
double VehicleLateralTire::lr()
{
	return lr_;
}
double VehicleLateralTire::cf()
{
	return cf_;
}
double VehicleLateralTire::cr()
{
	return cr_;
}
double VehicleLateralTire::iz()
{
	return iz_;
}
double VehicleLateralTire::max_tire_f_angle()
{
	return max_tire_f_angle_;
}
double VehicleLateralTire::speed_threshold()
{
	return speed_threshold_;
}

} // namespace control
} // namespace auto_ros